/*
 *  gda.cpp
 *  greedy-data-association
 *
 *  Created by Kris Kitani on 7/26/12.
 *  Copyright 2012 __MyCompanyName__. All rights reserved.
 *
 */

#include "gda.hpp"

void gda::init(string object_type, float score_t)
{
	

	FACTOR = 0.7;					// percent of image to use for training appearance model (whole image: 1.0, half image: 0.5)
	MIN_SAMPLES = 10;				// need atleast 10 sample to train a valid svm
	LOG_LIKELIHOOD_THRESH = -10.0;	// 
	SCORE_THRESH = score_t;			// SEE CLASS SPECIFIC SETTING!!!!
	T_CLOSE = 5;
	
	
	MAX_FRAMES = 100000;			// 1000(600) initial memory used to load file	
	PCA_DIMS = 50;					// PCA dimensions
	PCA_ON = 0;						// Use PCA (generally more robust, prevents overfitting)
	
	VIS_ON = 0;						// Visualzation flag
	VERBOSE = 0;					// Verbose cout flag
	PAUSE = 0;
	
	METHOD = "rtr";					// regressor
	
	MIN_W = 0;
	MIN_H = 0;
	
	if(object_type=="person")
	{
		//SCORE_THRESH = 0.13;		// 0.3 bigger value to avoid id switching
		FACTOR = 0.75;				// .85tighter bounding box for people ()this value affects per detection matching
		//if(METHOD=="rtr") SCORE_THRESH = 0.50; // 10.0 // 0.40 (keeps red girl as own) //
		MIN_W = 0.04;
		MIN_H = 0.32;
	}
	else if(object_type=="bag")
	{
		//SCORE_THRESH = 0.0;			// smaller value if there is only one bag
		FACTOR = 0.8;				// no need to tighten significantly
		PCA_DIMS = 25;	
	}
	
	
	min_gid =  INT_MAX;
	max_gid = -INT_MAX;
}


Rect gda::getTightBoundingBox(struct rect r, Mat &sm)
{
	float factor = FACTOR;
	int w = round( r.v[2]*factor*sm.cols*0.80);
	int h = round( r.v[3]*factor*sm.rows);
	int x = round( (r.v[0]-r.v[2]*0.50*factor) * sm.cols );
	int y = round( (r.v[1]+r.v[3]*0.50*(1.0-factor)) * sm.rows );
	if(x<0) x=0;
	//cout << x << ",";
	//cout << y << ",";
	//cout << w << ",";
	//cout << h << endl;
	return Rect(x,y,w,h);
}



void gda::run_gda(string video_path, string csv_path, string out_path, string object_type, float score_t)
{
	
	init(object_type,score_t);
	
	double scale = 0.5;
	
	BackgroundSubtractorMOG bm(500,5, 0.01, 10.0); // 320, 3, 0.001, 10.0 //320, 3, 0.01, 7.5 
	
	VideoCapture cap(video_path);
	if(!cap.isOpened()){
		cout << "ERROR: Cannot open avi: " << video_path << endl;
		exit(1);
	}
	
	int f=-1;
	Mat frm,sm,fg,mfg;
	
	vector <vector< struct rect > > ped(MAX_FRAMES);
	
	loadData_csv(csv_path, ped, timemax, object_type);
	
	vector<int> q;
	q.push_back(CV_IMWRITE_JPEG_QUALITY);
	q.push_back(100);
	
	
	timemax--;	// ignore last frame information
	
	
	// remove overlapping bounding boxes
	for(int t=0;t<timemax;t++)
	{
		if( (int)ped[t].size()<2 ) continue;
		
		//cout << "t:" << t << endl;
		
		for(int i=0;i<ped[t].size();i++)
		{
			for(int j=i+1;j<ped[t].size();j++)
			{
				//cout << "compare (i,j):" <<i << "," <<j << endl;
								
				float x_ul = MAX(ped[t][i].v[0]-ped[t][i].v[2]*0.5, ped[t][j].v[0]-ped[t][j].v[2]*0.5);
				float y_ul = MAX(ped[t][i].v[1], ped[t][j].v[1]);
				
				float x_br = MIN(ped[t][i].v[0]+ped[t][i].v[2]*0.5, ped[t][j].v[0]+ped[t][j].v[2]*0.5);
				float y_br = MIN(ped[t][i].v[1]+ped[t][i].v[3], ped[t][j].v[1]+ped[t][j].v[3]);
				
				float h = x_br - x_ul;
				float w = y_br - y_ul;
				
				// Check if true overlap
				if(h<0) continue;
				if(w<0) continue;
				
				float a = h*w;		// common area
				
				float a_i = ped[t][i].v[2]*ped[t][i].v[3];
				float a_j = ped[t][j].v[2]*ped[t][j].v[3];
				
				//float coverage_t = 0.30;		// over 70% overlap of either detection
				float size_ratio_t = 0.20;			// small box at least 20% of bigger box
				float bbox_overlap_t = 0.30;
				/*
				if( fabs(1.0 - MIN(a/a_i,a_i/a)) < coverage ||  fabs(1.0 - MIN(a/a_j,a_j/a)) < coverage  ){
					cout << "coverage_i:" << fabs(1.0 - MIN(a/a_i,a_i/a)) << endl;
					cout << "coverage_j:" << fabs(1.0 - MIN(a/a_j,a_j/a)) << endl;
					cout << "overlap ratio: " << MIN(a_i/a_j,a_j/a_i) << endl;
					cout << a << " i:"<<i<<":" << a_i << " j:" <<j <<":"<< a_j << endl;
					//cin.ignore();
				}
				 */
				
				float size_ratio = MIN(a_i/a_j,a_j/a_i);
				float bbox_overlap = MIN(a/a_j,a/a_j);
				
				if(VERBOSE){
					cout << "bbox_overlap:" << bbox_overlap << endl;
					if(bbox_overlap > 1.01){
						cout <<"area:" << a << " i:"<<i<<":" << a_i << " j:" <<j <<":"<< a_j << endl;
					}
				}
				
				if( (size_ratio > size_ratio_t) && (bbox_overlap>bbox_overlap_t) ){ // similar size & enough overlap
					if(a_j > a_i){
						ped[t].erase(ped[t].begin()+(j--));
					}
					else{
						ped[t].erase(ped[t].begin()+(i--));
						break;
					}
				}
				
				/*
				if( (fabs(1.0 - MIN(a/a_i,a_i/a)) < bbox_overlap_t) && (a_i/a_j > size_ratio_t) ){	// if complete overlap and comparable in size
					if(VERBOSE) cout << "remove i:" << i << " " << j << endl;
					ped[t].erase(ped[t].begin()+i);
					i--;
					break;
				}
				
				if( (fabs(1.0 - MIN(a/a_j,a_j/a)) < bbox_overlap_t) && (a_j/a_i > size_ratio_t) ){
					if(VERBOSE) cout << i << "-remove j:" << j << endl;
					ped[t].erase(ped[t].begin()+j);
					j--;
				}
				 */
				
			}
		}
	}
	
	//exit(1);
	//cin.ignore();
	
	z.clear();
	vector< vector<double> > vt(timemax);
	
	vector<double> v4(5,-1);
	
	for(int i=0;i<timemax;i++) vt[i] = v4;
	zt.v = vt;
	for(int i=0;i<timemax;i++) z.push_back(1);
	
	
	pc = vector<double>(2,0.5);
	if(METHOD=="dtr") dtr = vector<CvDTree>(1024);
	else if(METHOD=="rtr") rtr = vector<CvRTrees>(1024);
	
	// for some reason online alloction of classifiers does not work
	// try removing this in a later version of opencv
	
	
	J=0;
	
	int Tsum = 0;
	vector<struct rect> endpts;
	vector<struct rect> startpts;
	
	for(int t=0;t<timemax;t++)
	{
		if(VERBOSE) cout << "\n=========frame:" << t << "/" << timemax << " T.size()=" << (int)T.size()<< endl;
		if(VERBOSE) cout << "J=" << J << endl;
		
		f++;
		cap >> frm;
		if(!frm.data){
			if(t!=(timemax-1)){
				cout << "ERROR: More detections past the end of video actual video size." << endl;
				cout << "This video only has " << t << " frames but should have " << timemax << endl;
			}
			break;
		}
		
		resize(frm,sm,Size(),scale,scale);
		if((f%50)==0){
			cout << "frame:" << f << "/" << timemax << endl;
		}
		
		Mat gry;
		cvtColor(sm,gry,CV_RGB2GRAY);
		
		if(1){
			bm(sm,fg,-1);
		}
		else {
			cvtColor(sm,fg,CV_BGR2GRAY);
		}
		
		fg.convertTo(fg,CV_8UC1);
		medianBlur(fg,fg,3);
		medianBlur(fg,fg,5);
		
		Mat sim;
		
		resize(sm,sim,Size(),1.0,1.0);
		
		
		if(VERBOSE) cout << "Number of people detected in frame:" << (int)ped[t].size() << endl;
		
		
		//////////////////////////////////////////
		//										//
		//		COMPUTE APPEARANCE FEATURE		//
		//										//
		//////////////////////////////////////////
		
		for(int j=0;j<(int)ped[t].size();j++) // for each person in frame t
		{
			Rect r = getTightBoundingBox(ped[t][j],sm);
			Mat hist;
			//Mat msk = (fg(r)*0)+255;
			Mat msk = fg(r)*1.0;							// use background mask
			if(countNonZero(msk)<100){
				msk = 255;	// remove mask if no motion
			}
			computeColorHist_HS(sm(r),msk,hist);			// small histogram (frame to frame)
			ped[t][j].h = hist*1.0;							// update small histogram
		}
		
		
		if(Tsum==0 && (int)ped[t].size()==0)
		{
			if(VERBOSE) cout << "No detections yet." << endl;
		}
		//////////////////////////////////////////
		//										//
		//		  ADD FIRST DETECTIONS			//
		//										//
		//////////////////////////////////////////
		else if( Tsum==0 && (int)ped[t].size()>0)
		{	
			
			if(VERBOSE) cout << "Assign first set of detections " << (int)ped[t].size() << endl;
			
			for(int j=0;j<(int)ped[t].size();j++)
			{
				
				//if(ped[t][j].v[2] < )
					
				T.push_back(zt);			// initialize trajectory
				C.push_back(0);				// initialize class label
				Tj.push_back(0);			// initialize trajectory length
				logpDHj.push_back(1);		// likelihood per trajectory
				logpDHjt.push_back(z);		// likelihood per detection
				J++;
				
				struct rect p;
				p.v[0] = -1;
				p.v[1] = -1;
				p.v[2] = -1;
				p.v[3] = -1;
				p.t = -1;
				p.nomotion = 0;
				
				endpts.push_back(p);
				startpts.push_back(p);
				
				pos.push_back(Mat());
				neg.push_back(Mat());
				if(PCA_ON) pca.push_back(PCA());
				complete.push_back(0);
				//CvDTree dt;
				if(METHOD=="dtr") dtr.push_back(CvDTree());
				else if(METHOD=="rtr") rtr.push_back(CvRTrees());
				
				DA.push_back(-1);
				
				T[j].v[t][0] = ped[t][j].v[0];
				T[j].v[t][1] = ped[t][j].v[1];
				T[j].v[t][2] = ped[t][j].v[2];
				T[j].v[t][3] = ped[t][j].v[3];
				T[j].v[t][4] = ped[t][j].v[4];			// store pose id
				Tj[j]++;
				Tsum++;
				
				endpts[j].v[0] = ped[t][j].v[0];
				endpts[j].v[1] = ped[t][j].v[1];
				endpts[j].v[2] = ped[t][j].v[2];
				endpts[j].v[3] = ped[t][j].v[3];
				//endpts[j].v[4] = ped[t][j].v[4];
				endpts[j].t = t;
				endpts[j].h = ped[t][j].h * 1.0;
				endpts[j].gid = ped[t][j].gid;
				endpts[j].sum_w = ped[t][j].v[2];
				endpts[j].sum_h = ped[t][j].v[3];
				
				Rect r = getTightBoundingBox(ped[t][j], sm);
				
				endpts[j].i = sm(r) * 1.0;	// color image
				endpts[j].m = fg(r) * 1.0;	// mask
				endpts[j].nomotion += 0;
				
				if(countNonZero(fg(r))<100){
					endpts[j].m = 255;	// remove mask if no motion
					endpts[j].nomotion += 1;
				}
				
				startpts[j].v[0] = ped[t][j].v[0];
				startpts[j].v[1] = ped[t][j].v[1];
				startpts[j].v[2] = ped[t][j].v[2];
				startpts[j].v[3] = ped[t][j].v[3];
				startpts[j].t = t;
				//startpts[j].h = ped[t][j].h * 1.0;
				startpts[j].gid = ped[t][j].gid;
				
				Mat hist;
				computeColorHist_HSV(sm(r),endpts[j].m,hist);
				pos[j] = hist*1.0;							// big histogram for PCA
				//pos[j] = ped[t][j].h * 1.0;
				
				if(VIS_ON)
				{
					Point p = Point( (ped[t][j].v[0]-0.5*ped[t][j].v[2])*sim.cols,ped[t][j].v[1]*sim.rows);
					Point q = p + Point(ped[t][j].v[2]*sim.cols,ped[t][j].v[3]*sim.rows);
					rectangle(sim,p,q,Scalar::all(0),1,CV_AA);
					stringstream ss;
					ss << j;
					putText(sim,ss.str().c_str(),p,0,1.0,CV_RGB(255,255,0),1,CV_AA);
				}
			}
			
			
			// === ADD NEGATIVE DATA === //
			for(int j=0;j<(int)ped[t].size();j++)
			{
				for(int i=0;i<(int)ped[t].size();i++)
				{
					if(i==j) continue;
					if( countNonZero(endpts[i].m) == (endpts[i].m.rows * endpts[i].m.cols) ) continue;
					
					if( neg[j].data==NULL ) neg[j] = pos[i]*1.0;
					else neg[j].push_back(pos[i]);
				}
				
				Rect r = getTightBoundingBox(ped[t][j],sm);
				
				//if(VIS_ON) imshow("sub",sm(r));
				
				// RIGHT
				if( (r.x+r.width*2)<sm.cols)
				{
					Rect re = r;
					re.x += re.width;
					Mat hist;
					
					Mat msk = fg(re)*1.0;
					if(countNonZero(msk)<100) msk = 255;
						
					computeColorHist_HSV(sm(re),msk,hist);
					neg[j].push_back(hist);
					//if(VIS_ON) imshow("right",sm(re));
				}
				
				
				// === LEFT
				if( (r.x-r.width)>=0)
				{
					Rect re = r;
					re.x -= re.width;
					Mat hist;
					//Mat msk = (fg(re) * 0) + 255;
					
					Mat msk = fg(re)*1.0;
					if(countNonZero(msk)<100) msk = 255;

					computeColorHist_HSV(sm(re),msk,hist);
					neg[j].push_back(hist);
					//if(VIS_ON) imshow("left",sm(re));
				}
			}
			// ============================ //
			
			
			if(VERBOSE) for(int j=0;j<(int)T.size();j++)
				cout << "FIRST: pos points=" << pos[j].rows << " neg points=" << neg[j].rows << endl;
			
		}
		//////////////////////////////////////////
		//										//
		//			ADD NEW DETECTIONS			//
		//										//
		//////////////////////////////////////////
		else{
			
			vector<int> statj;
			for(int j=0;j<(int)T.size();j++) statj.push_back(-1);
			
			vector<int> statk;
			for(int k=0;k<(int)ped[t].size();k++) statk.push_back(-1);
			
			int assigned = 0;
			
			Mat hist[(int)ped[t].size()];
			
			if(VERBOSE) cout << "=== Compute Greedy Assignments ===" << endl;
			
			while(assigned<(int)ped[t].size())
			{
				int idj = -1;
				int idk = -1;
				
				int lonek = -1;
				
				double mindist = LOG_LIKELIHOOD_THRESH;
				
				for(int k=0;k<(int)ped[t].size();k++){						
					
					if(statk[k]>=0) continue; // already assigned
					lonek = k;
					
					for(int j=0;j<J;j++)
					{
						if(statj[j]>=0) continue; // trajectory j has already been assigned
						if(complete[j]==1) continue;
						
						double x,_x,y,_y,w,_w,h,_h;
						int _t;
						Mat a, _a;
						int gid, _gid;
						
						x = ped[t][k].v[0];
						y = ped[t][k].v[1];
						w = ped[t][k].v[2];
						h = ped[t][k].v[3];
						a = ped[t][k].h;
						gid = ped[t][k].gid;
						
						_x = endpts[j].v[0];
						_y = endpts[j].v[1];
						_w = endpts[j].v[2];
						_h = endpts[j].v[3];
						_t = endpts[j].t;
						_a = endpts[j].h;
						_gid = endpts[j].gid;
						
						double dist=0;						
						dist += getLogPdt(t,_t,0);
						dist += getLogPss(w,_w,h,_h,0);
						dist += getLogPxx(x,_x,y,_y,t,_t,0);
						dist += getLogPaa(a,_a);
						//dist += getLogPgid(gid, _gid);
						
						//if( (endpts[j].t-startpts[j].t)==100) dist = -FLT_MAX;
						//if(gid==_gid) dist = 0;					// this forces all incoming tracks to remain together no matter what!!!
						//else dist = -FLT_MAX;
						
						/*
						if(VERBOSE)// if(dist>LOG_LIKELIHOOD_THRESH)
						{
							cout << "k=" <<k << " j=" <<j << " "; 
							cout << "  logpdt("<<t-_t<<")=" << getLogPdt(t,_t,0);
							cout << "  logpss=" << getLogPss(w,_w,h,_h,0);
							cout << "  logpxx=" << getLogPxx(x,_x,y,_y,t,_t,0);
							cout << "  logpaa=" << getLogPaa(a,_a);
							cout << "  logpgid=" << getLogPgid(gid,_gid);
							cout << "  dist=" << dist << endl;
						}
						 */
						
						
						if( dist > mindist ){
							mindist = dist;
							idj = j;	// best pair
							idk = k;	// best pair
						}
					}
				}
				
				if(VERBOSE) cout << "Best pair (interm):" << idj << " " << idk << " mindist(log):" << mindist << endl; 
				
				//////////////////////////////////
				//								//
				//	   Start New Trajectory		//
				//								//
				//////////////////////////////////
				if(mindist==LOG_LIKELIHOOD_THRESH)
				{
					
					if(VERBOSE){
						cout << "Could not find good match." << endl;
						cout << "======= starting a NEW trajectory!!! before adding " << (int)T.size()  << endl;
					}
					
					if(idk==-1) idk = lonek;
					
					T.push_back(zt);			// initialize trajectory
					C.push_back(0);				// initialize class label
					Tj.push_back(0);			// initialize trajectory length
					logpDHj.push_back(1);		// likelihood per trajectory
					logpDHjt.push_back(z);		// likelihood per detection
					J++;						// increment num trajectories
					
					struct rect p;
					p.v[0] = -1;
					p.v[1] = -1;
					p.v[2] = -1;
					p.v[3] = -1;
					p.t = -1;
					p.nomotion = 0;
					
					endpts.push_back(p);
					startpts.push_back(p);
					
					statj.push_back(-1);
					idj = (int)T.size()-1;
					
					startpts[idj].v[0] = ped[t][idk].v[0];
					startpts[idj].v[1] = ped[t][idk].v[1];
					startpts[idj].v[2] = ped[t][idk].v[2];
					startpts[idj].v[3] = ped[t][idk].v[3];
					startpts[idj].t = t;
					startpts[idj].gid = ped[t][idk].gid;
					
					pos.push_back(Mat());
					neg.push_back(Mat());
					if(PCA_ON) pca.push_back(PCA());
					complete.push_back(0);
					//CvDTree dt;
					if(METHOD=="dtr") dtr.push_back(CvDTree());
					else if(METHOD=="rtr") rtr.push_back(CvRTrees());
					DA.push_back(-1);
					
					
				}
				
				statj[idj]=idk;
				statk[idk]=idj;
				
				if(VERBOSE) cout << "mindist=" << mindist << " (k="<<idk<<"/"<< (int)ped[t].size()<<",assigned to j="<<idj<<")"<<  endl;
				
				assigned++;
				
				// (1) add trajectory
				T[idj].v[t][0] = ped[t][idk].v[0];
				T[idj].v[t][1] = ped[t][idk].v[1];
				T[idj].v[t][2] = ped[t][idk].v[2];
				T[idj].v[t][3] = ped[t][idk].v[3];
				T[idj].v[t][4] = ped[t][idk].v[4];		// store pose id
				Tj[idj]++;
				Tsum++;
				
				// (2) update end-point 'j'
				endpts[idj].v[0] = ped[t][idk].v[0];
				endpts[idj].v[1] = ped[t][idk].v[1];
				endpts[idj].v[2] = ped[t][idk].v[2];
				endpts[idj].v[3] = ped[t][idk].v[3];
				endpts[idj].t = t;
				endpts[idj].h = ped[t][idk].h * 1.0;
				endpts[idj].gid = ped[t][idk].gid;
				endpts[idj].sum_w += ped[t][idk].v[2];
				endpts[idj].sum_h += ped[t][idk].v[3];
				

				
				Rect r = getTightBoundingBox(ped[t][idk],sm);
				
				endpts[idj].i = sm(r) * 1.0;	// update last known color image
				endpts[idj].m = fg(r) * 1.0;	// update last known color image
				endpts[idj].nomotion += 0;		// 
				
				if(countNonZero(fg(r))<100){
					endpts[idj].m = 255;		// remove mask if no motion
					endpts[idj].nomotion += 1;
				}
				
				// (3) store positive example
				Mat his;
				computeColorHist_HSV(sm(r),endpts[idj].m,his);
				//hist[idk] = ped[t][idk].h * 1.0;
				hist[idk] = his * 1.0;
				
				if(!pos[idj].data) pos[idj] = hist[idk]*1.0;
				else pos[idj].push_back(hist[idk]);
				
				computeLikelihood();
				
				//visualizeAllTrajectories();
				
				if(1)
				{
					Point p = Point( (ped[t][idk].v[0]-0.5*ped[t][idk].v[2])*sim.cols,ped[t][idk].v[1]*sim.rows);
					Point q = p + Point(ped[t][idk].v[2]*sim.cols,ped[t][idk].v[3]*sim.rows);
					rectangle(sim,p,q,Scalar::all(255),1,CV_AA);
					stringstream ss;
					ss << idj;
					putText(sim,ss.str().c_str(),q,0,1.0,CV_RGB(255,255,0),1,CV_AA);
				}
				
			} // assign to trajectory
			
			
			
			//////////////////////////////////////
			//									//
			//		ADD NEGATIVE EXAMPLES		//
			//									//
			//////////////////////////////////////
			
			for(int j=0;j<(int)ped[t].size();j++)
			{
				// Add other objects in same time frame
				int idj = statk[j];
				for(int i=0;i<(int)ped[t].size();i++){
					if(j==i) continue;
					if( countNonZero(endpts[i].m) == (endpts[i].m.rows * endpts[i].m.cols) ) continue; // don't add no motion images
					if(neg[idj].data==NULL) neg[idj] = hist[i]*1.0;
					else neg[idj].push_back(hist[i]); 
				}
				
				Rect r = getTightBoundingBox(ped[t][j],sm);
				//if(VIS_ON) imshow("sub",sm(r));
				
				if( (r.x+r.width*2)<sm.cols)	// RIGHT
				{
					Rect re = r;
					re.x += re.width;
					Mat hst;
					Mat msk = (fg(re) * 0) + 255;
					computeColorHist_HSV(sm(re),msk,hst);
					neg[idj].push_back(hst);
					//if(VIS_ON) imshow("right",sm(re));
				}
				
				
				if( (r.x-r.width)>0 )			// LEFT
				{
					Rect re = r;
					re.x -= re.width;
					Mat hst;
					Mat msk = (fg(re) * 0) + 255;
					computeColorHist_HSV(sm(re),msk,hst);
					neg[idj].push_back(hst);
					//if(VIS_ON) imshow("left",sm(re));
				}
			}
			
			
			if(VERBOSE) for(int j=0;j<(int)T.size();j++)
			{
				cout << j << "] pos points=" << pos[j].rows << " neg points=" << neg[j].rows;
				if(t-endpts[j].t>T_CLOSE) cout << "(closed)";
				if(complete[j]==1){
					cout << "(complete/trained)";
				}
				cout << " t_start:" << startpts[j].t;
				cout << " t_end:"   << endpts[j].t;
				cout << endl;
				
				if(t-endpts[j].t<T_CLOSE && complete[j]==1){
					cout <<"t:"<< t << ", dt:"<< t-endpts[j].t << " frames (less than "<< T_CLOSE <<") since end (" << endpts[j].t << ".";
					cout << "training should not be done yet for trajectory " <<j << endl;
					exit(1);
				}
			}
			

			
		} // assigned detection to trajectory (new or old)
		
		
		
		
		if( (int)T.size() != (int)logpDHj.size() ){
			cout << "ERROR: Inconsistent num of trajectories:" << (int)T.size() << " " << (int)logpDHj.size() << endl;
			exit(1);
		}
		if( (int)T.size() != (int)logpDHjt.size() ){
			cout << "ERROR: Inconsistent num of trajectories:" << (int)T.size() << "!=" << (int)logpDHjt.size() << endl;
			exit(1);
		}
		
		
		
		//////////////////////////////////////////
		//										//
		//		TRAIN APPEARANCE MODEL			//
		//										//
		//////////////////////////////////////////
		
		for(int j=0;j<(int)T.size();j++)
		{
			int _t = endpts[j].t;
			int dt = t-_t;
			
			if(0){	// Propagate if near duplicate
				//if( (dt>0) && (dt<30) && (pca[j].mean.data==NULL) && (pos[j].rows>10) )
				//if( (dt>0) && (dt<15) && (pca[j].mean.data==NULL) )
				if( (dt>0) && (dt<15) && (complete[j]==0) )
				{ 
					
					
					Rect r = getTightBoundingBox(ped[t][j],sm);
					
					Mat last = endpts[j].i; // color image
					Mat curr = sm(r);
					
					double score = pow(norm(last,curr,NORM_L2),2) / ( norm(last,NORM_L2)*norm(curr,NORM_L2) );
					
					if( (score < 0.05) && (score>0.0005) )
					{
						endpts[j].t = t;		// update time
						endpts[j].i = curr*1.0; // update image
						
						T[j].v[t][0] = T[j].v[_t][0];
						T[j].v[t][1] = T[j].v[_t][1];
						T[j].v[t][2] = T[j].v[_t][2];
						T[j].v[t][3] = T[j].v[_t][3];
						T[j].v[t][4] = -1; // don't propagate pose
						
						// ==== visualize ==== //
						if(1){
							Point p = Point( (endpts[j].v[0]-0.5*endpts[j].v[2])*sim.cols,endpts[j].v[1]*sim.rows);
							Point q = p + Point(endpts[j].v[2]*sim.cols,endpts[j].v[3]*sim.rows);
							rectangle(sim,p,q,CV_RGB(255,0,0),1,CV_AA);
							stringstream ss;
							ss << j;
							putText(sim,ss.str().c_str(),p, 0,1.0,CV_RGB(255,0,0),1,CV_AA);
						}
					}
				}
			}
			
			
			
			
			//////////////////////////////////////////
			//										//
			//			CLOSING TRAJECTORY			//
			//										//
			//////////////////////////////////////////
			
			if(VERBOSE) cout << "===========" << j << "==============" << endl;
			if(VERBOSE) cout << "average width of " << j << " is " << endpts[j].sum_w/pos[j].rows << endl;
			if(VERBOSE) cout << "average height of " << j << " is " << endpts[j].sum_h/pos[j].rows << endl;
			//cin.ignore();
			if(VERBOSE) cout << "no motion count:" << endpts[j].nomotion << endl;
			
			int no_motion_thresh = 0;
			if( ((endpts[j].nomotion>no_motion_thresh)||(dt==T_CLOSE)||(t==(timemax-1))) && (complete[j]==0)) // added no motion removal here!!!
			{
				if(VERBOSE) cout << "Closing trajectory. ended at:" << endpts[j].t << " currently " << t << endl;
				
				if(t==(timemax-1)) cout << "END OF TIME" << endl;
				
				//////////////////////////////////////////
				//										//
				//			REMOVE TRAJECTORY			//
				//										//
				//////////////////////////////////////////
				
				if(VERBOSE) if( (endpts[j].sum_h/pos[j].rows<MIN_H) || (endpts[j].sum_w/pos[j].rows<MIN_W)){
					cout << "=============== TO SMALL!!!! ===============" << endl;
				}
					
				// TODO: Remove trajectories with no motion (false detections)
				if( (pos[j].rows<MIN_SAMPLES) 
				   || (neg[j].rows<MIN_SAMPLES) 
				   || (endpts[j].nomotion>no_motion_thresh) 
				   || (endpts[j].sum_h/pos[j].rows<MIN_H) 
				   || (endpts[j].sum_w/pos[j].rows<MIN_W) )
				{
					if(VERBOSE){
						cout << "==== Removing Trajectory " << j << endl;
						if(endpts[j].nomotion>no_motion_thresh) cout << "No motion for :" << endpts[j].nomotion << " frames.\n";
						
						//if(PAUSE){
						//	imshow("dummy",Mat::zeros(10,10,CV_8UC1));
						//	waitKey(0);
						//}
					}
					
					T.erase(T.begin()+j);
					C.erase(C.begin()+j);
					Tj.erase(Tj.begin()+j);
					logpDHj.erase(logpDHj.begin()+j);
					logpDHjt.erase(logpDHjt.begin()+j);
					endpts.erase(endpts.begin()+j);
					startpts.erase(startpts.begin()+j);
					
					pos.erase(pos.begin()+j);
					neg.erase(neg.begin()+j);
					if(PCA_ON) pca.erase(pca.begin()+j);
					complete.erase(complete.begin()+j);
					
					if(METHOD=="dtr") dtr.erase(dtr.begin()+j);
					else if(METHOD=="rtr") rtr.erase(rtr.begin()+j);
					DA.erase(DA.begin()+j);
					
					J = (int)T.size();
					j--;				// decrement counter to avoid skipping
					continue;
				}
				
				//////////////////////////////////////////
				//										//
				//			   TRAIN MODELS				//
				//										//
				//////////////////////////////////////////
				

				
				// subsample positive and negative examples if very big!
				//if(neg[j].rows>500){
				//	neg[j] = neg[j].rowRange(0,500);
				//}
				
				if(VERBOSE){
					cout << " ===== Training model "<< j << " of " << J << endl;
					cout << "Train. Pos: " << pos[j].rows << " Neg:" << neg[j].rows << endl;
				}
				
				Mat mTrain;
				pos[j].copyTo(mTrain);
				mTrain.push_back(neg[j]);
				
				Mat mLabels = Mat::ones(mTrain.rows,1,CV_32FC1);	// Make 1,-1 labels
				mLabels.rowRange(0,pos[j].rows) = 1.0;				// positive examples
				mLabels.rowRange(pos[j].rows,mLabels.rows) = -1.0;	// negative examples
				CvMat trainClass = mLabels;
				
				double tt;
				if(PCA_ON)
				{
					cout << "Running PCA ...";
					tt = getTickCount();
					pca[j](mTrain,Mat(),CV_PCA_DATA_AS_ROW,PCA_DIMS);			// 20 dims
					tt = (getTickCount()-tt)/getTickFrequency();
					cout << " time:" << tt << " secs." << endl;
					
					mTrain = pca[j].project(mTrain);
					if(VERBOSE) cout << "c,r[" << mTrain.cols << " " << mTrain.rows << endl;
				}
				
				CvMat trainData = mTrain;
				
				if(METHOD=="dtr")
				{
					CvDTreeParams dt_params = CvDTreeParams();
					dt_params = CvDTreeParams(12,2,0.0,true,2,1,true,true,0);
					if(VERBOSE) cout << "num data rows:" << mTrain.rows << " cols:" << mTrain.cols << endl;
					Mat varType = Mat::ones(mTrain.cols+1,1,CV_8UC1) * CV_VAR_NUMERICAL; // all floats
					
					cout << "Training decision tree model ...";
					tt = getTickCount();
					dtr[j].train(mTrain,CV_ROW_SAMPLE,mLabels,Mat(),Mat(),varType,Mat(),dt_params);
					tt = (getTickCount()-tt)/getTickFrequency();
					cout << " time:" << tt << " secs." << endl;
				}
				else if(METHOD=="rtr")
				{
					CvRTParams params = CvRTParams();
					
					params.max_depth = 50;
					params.regression_accuracy = 0.01;
					params.use_1se_rule = true;
					params.use_surrogates = true;
					params.truncate_pruned_tree = true;
					params.min_sample_count = 10;
					//params.max_num_of_trees_in_the_forest = 10;
					
					if(VERBOSE) cout << "num data rows:" << mTrain.rows << " cols:" << mTrain.cols << endl;
					Mat varType = Mat::ones(mTrain.cols+1,1,CV_8UC1) * CV_VAR_NUMERICAL; // all floats
					varType.at<uchar>(mTrain.cols,0) = CV_VAR_CATEGORICAL;
					
					cout << "Training decision tree model ...";
					tt = getTickCount();
					rtr[j].train(mTrain,CV_ROW_SAMPLE,mLabels,Mat(),Mat(),varType,Mat(),params);
					tt = (getTickCount()-tt)/getTickFrequency();
					cout << " time:" << tt << " secs." << endl;
				}
				
				complete[j] = 1;		// performed training
				
				
				//////////////////////////////////////////////////
				//												//
				//		M E R G E   T R A J E C T O R I E S		//
				//												//
				//////////////////////////////////////////////////
				
				if(VERBOSE) cout << "==== MERGE TRAJECTORIES ====" << endl;
				
				double score_max = -2.0;
				int dt = 0;
				int delta = 0;
				
				int idx = -1;
				int jdx = -1;
				
				for(int i=0;i<(int)T.size();i++)
				{
					
					
					//if(endpts[i].t>startpts[j].t)
					if( endpts[i].t > (startpts[j].t+T_CLOSE) )
					{
						if(VERBOSE) cout << i << ") Not from the past! endpt(i):" << endpts[i].t << " > start(j)+T_CLOSE:" << startpts[j].t << endl;
						continue;			// only compare to trajectories of the past
					}
					
					if( complete[i]==0 || complete[j]==0 ) continue;	// complete trajectories only
					if( i==j ) continue;								// no self comparision
					
					float cscore = 0;
					
					// === HEAVY-SIDE STEP-FUNCTION === //
					
					//cout << "compute dist " << j << " and " << i << endl;
					// test j on i
					for(int n=0;n<pos[i].rows;n++)
					{				
						Mat sample;
						if(PCA_ON) sample = pca[j].project(pos[i].row(n));
						else sample = pos[i].row(n)*1.0;
						
						if(METHOD=="dtr"){
							CvDTreeNode *node;
							node = dtr[j].predict(sample);
							cscore += node->value * 1./pos[i].rows;			// normalize score
							//if(node->value > score) score = node->value;
						}
						else if(METHOD=="rtr"){
							cscore += rtr[j].predict(sample)*1./pos[i].rows;
						}
					}
					
					// test i on j
					for(int n=0;n<pos[j].rows;n++)
					{
						Mat sample;
						if(PCA_ON) sample = pca[i].project(pos[j].row(n));
						else sample = pos[j].row(n)*1.0;
						
						sample.convertTo(sample,CV_32FC1);
						if(METHOD=="dtr"){
							CvDTreeNode *node;
							node = dtr[i].predict(sample);
							cscore += node->value * 1./pos[j].rows;			// normalize score
							//if(node->value > score) score = node->value;
						}
						else if(METHOD=="rtr"){
							cscore += rtr[i].predict(sample)* 1./pos[j].rows;
						}
					}
					
					// TODO: Don't associate to tracks that have exited
					dt = startpts[j].t - endpts[i].t;
					
					if( cscore>score_max )
					{
						score_max = cscore;
						idx=i;
						jdx=j;
						delta = dt;
					}
					
					if(VERBOSE) cout << "score("<< i <<","<<j<<"): "<<cscore<<endl;
					
				}
				
				if(VERBOSE){
					cout << "Should I merge trajectory " << j << " with old traj " << idx << " SCORE_MAX:" << score_max << " (dt:"<< delta << ")"<< endl;
					if(PAUSE) waitKey(0);
				}
				
				if(idx==-1 || jdx==-1) continue;
				
				if(score_max < SCORE_THRESH) continue;
				
				if(VERBOSE){
					cout << "YES: Merge trajectory " << jdx << " with old traj " << idx << " SCORE_MAX:" << score_max << ")" << endl;
					if(PAUSE) waitKey(0);
				}
				
				DA[jdx] = idx;
				
				
			}
		} // Train appearance model and associate
		
		
		
		if(VIS_ON)
		{
			Mat sfg;
			resize(fg,sfg,Size(),0.5,0.5);
			imshow("fg",sfg);
			//imshow("src",sm);
			imshow("sim",sim);
			
			int pw = 60;	// person width
			int ph = 100;	// person height
			int pc = 6;		// num columns
			int pr = MAX( ceil(T.size()*1.0/pc),1);
			
			Mat dsp = Mat::ones(ph*pr,pw*pc,CV_8UC3);
			rectangle(dsp,Point(0,0),Point(dsp.cols,dsp.rows),Scalar::all(255),-1);
			
			
			for(int i=0;i<(int)T.size();i++)
			{
				Mat im = endpts[i].i;		// image
				Mat ms = endpts[i].m;		// mask
				
				resize(im,im,Size(pw,ph),0,0);
				resize(ms,ms,Size(pw,ph),0,0);

				int x = i%pc;
				int y = floor(i/pc);
				
				Rect r(x*pw,y*ph,pw,ph);
				
				Mat sub = dsp(r);
				
				if(0) im.copyTo(sub,ms);
				else{
					im.copyTo(ms,ms);
					//cvtColor(ms,ms,CV_GRAY2BGR);
					addWeighted(im,0.7,ms,0.3,0,sub);
				}
				
				if(t==timemax-1)
				{
					Mat imout = sub * 1.0;
					stringstream ss;
					ss << i;
					putText(imout,ss.str().c_str(),Point(5,15),1,1.0,CV_RGB(255,255,255),2,CV_AA);
					putText(imout,ss.str().c_str(),Point(5,15),1,1.0,CV_RGB(0,0,0),1,CV_AA);
					if(DA[i]>=0){
						ss.str("");
						ss << DA[i];
						putText(imout,ss.str().c_str(),Point(45,90),1,1.0,CV_RGB(255,255,255),2,CV_AA);
						putText(imout,ss.str().c_str(),Point(45,90),1,1.0,CV_RGB(0,0,0),1,CV_AA);
						
					}
					ss.str("");
					ss << i;
					if(DA[i]>=0) ss << "-" << DA[i];
					else ss << "-root";
					ss << ".jpg";
					imwrite(ss.str(),imout,q);
					
				}
				
				Scalar color = CV_RGB(255,255,0);
				if(complete[i]==1) color = CV_RGB(255,0,0);
				
				stringstream text;
				text << i;
				
				if(DA[i]<0 && complete[i]==1) putText(sub,text.str().c_str(),Point(15,50),2,1.0,CV_RGB(0,255,0),2,CV_AA); // first instance
				else if(complete[i]==0 && DA[i]<0) putText(sub,text.str().c_str(),Point(8,50),2,1.3,CV_RGB(255,0,0),2,CV_AA);
				else{
					putText(sub,text.str().c_str(),Point(12,50),2,1.0,CV_RGB(0,0,0),2,CV_AA);
					putText(sub,text.str().c_str(),Point(12,50),2,1.0,CV_RGB(255,255,0),1,CV_AA);
				}
				if(DA[i]>=0){
					text.str("");
					text << DA[i];
					putText(sub,text.str().c_str(),Point(15,80),2,1.0,CV_RGB(0,0,0),2,CV_AA);
					putText(sub,text.str().c_str(),Point(15,80),2,1.0,CV_RGB(255,255,255),1,CV_AA);
				}
				
			}
			imshow("dsp",dsp);
			waitKey(1);
			
			if(t==timemax-1){
				imwrite("data-association.jpg",dsp,q);
				cout << "Total number of trajectories:" << (int)T.size() << endl;
			}
		}
	} // For t
	
	_T = T;
	
	
	
	
	
	
	//////////////////////////////////
	//								//
	//		 MERGE or REMOVE		//
	//								//
	//////////////////////////////////
	
	for(int j=(int)_T.size()-1;j>=0;j--)
	{
		int idx = DA[j];
		
		if(VERBOSE)
		{
			cout << j;
			cout << " pos:" << pos[j].rows;
			cout << " neg:" << neg[j].rows;
			cout << " idx:" << idx;
			cout << endl;
		}
		
		if(complete[j]==0)
		{
			if(VERBOSE) cout << "Not complete. Remove " << j << "-th trajectory."<< endl;
			_T.erase(_T.begin()+j);
		}
		else if(idx>=0)
		{ // merge trajectories, and remove
			if(VERBOSE) cout << j << " associated with (to be merged with) " << idx << endl;
			
			for(int t=startpts[j].t;t<endpts[j].t+1;t++)
			{
				if(_T[idx].v[t][2]<=0)
				{
					_T[idx].v[t][0] = _T[j].v[t][0];
					_T[idx].v[t][1] = _T[j].v[t][1];
					_T[idx].v[t][2] = _T[j].v[t][2];
					_T[idx].v[t][3] = _T[j].v[t][3];
					_T[idx].v[t][4] = _T[j].v[t][4];		// store pose id
				}
				else {
					cout << "overlap while merging, keep old trajectory." << endl;
				}
			}
			
			// update
			startpts[idx].t = min(startpts[j].t,startpts[idx].t);
			endpts[idx].t   = max(endpts[j].t,endpts[idx].t);
			endpts[idx].gid = min(endpts[j].gid,endpts[idx].gid); // take the smaller id number
			
			_T.erase(_T.begin()+j);
		}
		else
		{
			if(VERBOSE) cout << "Keep trajectory " << j << endl;
		}
		
		T = _T;
		computeLikelihood();
		//visualizeAllTrajectories();
	}
	
	cout << "After merging and removing trajectories: " << (int)T.size() << "trajectories left." << endl;
	
	
	//////////////////////////////////
	//								//
	//		 POST-PROCESSING		//
	//								//
	//////////////////////////////////
	
	if(VERBOSE) cout << "post processing: interpolation" << endl;
	// (1) small gap interpolation
	int smallgap = 10;
	
	for(int j=0;j<(int)T.size();j++)
	{
		
		int etime = 0;
		int stime = 0;
		//float dx[2];
		//float dy[2];
		
		for(int t=1;t<timemax-1;t++)
		{
			
			if(T[j].v[t-1][2]>0 && T[j].v[t][2]<=0){
				stime = t-1;				// store start of gap
				etime = 0;					// reinitialize end of gap
				//dx[0] = T[j].v[t-1][1];		// center x
				//dy[0] = T[j].v[t-1][2];		// upper left y
			}
			else if(T[j].v[t-1][2]<=0 && T[j].v[t][2]>0){
				etime = t;				// store end of gap
				//dx[1] = T[j].v[t][1];	// center x
				//dy[1] = T[j].v[t][2];	// upper left y

			}
			else{
				
			}
			
			
			if( ((etime-stime)>0) &&			// temporally ordered
			    ((etime-stime)<smallgap) &&		// gap is smaller than small gap
			    (stime!=0)  ){					// 
				
				float dt = etime-stime;
				
				float dx = T[j].v[etime][0] - T[j].v[stime-1][0];
				
				cout <<"j:" << j << ",t:" << t << " dx=" << dx << endl;
				
				//if (dx>0.005){
				// if motion is more than 10% motion 
				if (fabs(dx)>0.01){ // 0.005 dont interpolate if too much motion (forgot to take absolute value)
					etime = 0;
					stime = 0;
					continue;
				}
				
				if(VERBOSE) cout << "[INTERPOLATING GAP] between stime:" << stime << " endtime:" << etime << endl;
				
				float dy = T[j].v[etime][1] - T[j].v[stime-1][1];
				float dw = T[j].v[etime][2] - T[j].v[stime-1][2];
				float dh = T[j].v[etime][3] - T[j].v[stime-1][3];
				
				for(int s=1;s<dt;++s){
					T[j].v[stime+s][0] = T[j].v[stime][0] + s * dx/dt;
					T[j].v[stime+s][1] = T[j].v[stime][1] + s * dy/dt;
					T[j].v[stime+s][2] = T[j].v[stime][2] + s * dw/dt;
					T[j].v[stime+s][3] = T[j].v[stime][3] + s * dh/dt;
					T[j].v[stime+s][4] = -1;							// no pose id for interpolated points
				}
			}
			if(VERBOSE) if(T[j].v[t][2]>0) cout <<"======== ("<< j << "th traj) t="<< t << " " << T[j].v[t][2] << " stime:" << stime << " endtime:" << etime << endl;
		}
	}
	
	_T = T;
	
	cout << "After postprocessing: " << (int)T.size() << "trajectories left." << endl;

	
	writeToFile_csv(out_path,object_type);
	
	if(VERBOSE) cout << "Deallocating T" << endl;
	T.clear();
	if(VERBOSE) cout << "Deallocating _T" << endl;
	_T.clear();
	if(VERBOSE) cout << "Deallocating C" << endl;
	C.clear();
	if(VERBOSE) cout << "Deallocating Tj" << endl;
	Tj.clear();
	
	if(VERBOSE) cout << "Deallocating logDHjt" << endl;
	for(int i=0;i< (int)logpDHjt.size();i++) logpDHjt[i].clear();	
	logpDHjt.clear();
	
	if(VERBOSE) cout << "Deallocating logDHj" << endl;
	logpDHj.clear();				// likelihood per trajectory
	
	if(VERBOSE) cout << "Deallocating pc" << endl;
	pc.clear();
	
	if(VERBOSE) cout << "Deallocating z" << endl;
	z.clear();
	
	if(PCA_ON) pca.clear();
	
	if(VERBOSE) cout << "Deallocating pos" << endl;
	for(int i=0;i<(int)pos.size();i++) pos[i].release();
	pos.clear();
	
	if(VERBOSE) cout << "Deallocating neg" << endl;
	for(int i=0;i<(int)neg.size();i++) neg[i].release();
	neg.clear();
	
	if(VERBOSE) cout << "Deallocating classifier" << endl;
	//for(int i=0;i<(int)dtr.size();i++) dtr[i].clear();
	if(METHOD=="dtr") dtr.clear();
	else if(METHOD=="rtr") rtr.clear();
	
	if(VERBOSE) cout << "Deallocating DA" << endl;
	DA.clear();
	
	if(VERBOSE) cout << "Done. Press any key to quit." << endl;
	
	return;
}

double gda::getLogPs(double w){
	double sigma = 0.05; // 0.01
	double ps = exp( sqrt(pow(w-0.05,2)) / (-2*pow(sigma,2)) );
	return log(ps);
}

double gda::getLogPss(double w,double _w, double h,double _h, int cj){
	
	double sig[2];
	sig[0] = 0.5; //0.08
	sig[1] = 0.5; // fp, scale doesn't change
	double wratio = w/_w;
	double pss = exp( sqrt( pow(wratio-1.0,2)) / (-2*pow(sig[cj],2)) );
	if(pss== -INFINITY) if(VERBOSE) cout << "  pss("<< w << "|"<< _w <<")=" << pss << " (" << log(pss) << endl;
	return log(pss);
}

double gda::getLogPxx(double x, double _x,double y,double _y,int t,int _t,int cj){
	
	double sig[2];
	sig[0] = 0.08; // 0.5
	sig[1] = 0.05; // 0.2 fp, scale doesn't change
	
	double pxx = exp( sqrt(pow(x-_x,2)+pow(y-_y,2)) / (-2*pow(sig[cj],2)) );
	if(pxx== -INFINITY) if(VERBOSE) cout << "  pxx("<< x << "|"<< _x <<")=" << pxx << " (" << log(pxx) << endl;
	return log(pxx);
}

double gda::getLogPaa(Mat &a, Mat &_a){
	double sig = 1.0;
	double dist = compareHist(a,_a, CV_COMP_CHISQR); // histogram intersection dist
	double paa = exp( sqrt( pow(dist,2)) / (-2*pow(sig,2)) );
	if(paa== -INFINITY) if(VERBOSE) cout << "  paa("<< dist <<")=" << paa << " (" << log(paa) << endl;
	return log(paa);
	//return 0;
}

double gda::getLogPdt(int t,int _t,int cj){
	double dt = t-_t;
	double dtsig[2];
	dtsig[0]= 0.5; // 0.5
	dtsig[1]= 0.8; //0.8
	double pdt = exp(  sqrt(pow(dt-1.0,2))   /   (-2*pow(dtsig[cj],2))    );
	if(pdt== -INFINITY) if(VERBOSE) cout << "  pdt("<< dt <<")=" << pdt << " (" << log(pdt) << endl;
	return log(pdt);
}

double gda::getLogPgid(int gid,int _gid)
{
	if(gid==_gid) return 0;
	else return -INFINITY;
}

void gda::visualizeAllTrajectories(){
	
	if(!VIS_ON) return;
	
	if((int)T.size()==0) return;
	//int w = round(840.0/T.size());
	//double h = 480;
	int w = round(1080.0/(int)T.size());
	double h = 720;
	Mat dsp = Mat::zeros(h,w*(int)T.size(),CV_8UC3);
	for(int j=0;j<(int)T.size();j++)
	{
		//if(Tj[j]==0) rectangle(dsp,Point(w*j,0),Point(w*(j+1),dsp.rows),Scalar::all(100),-1,CV_AA);
		//if(C[j]==1) rectangle(dsp,Point(w*j,0),Point(w*(j+1),dsp.rows*0.5),Scalar::all(150),-1,CV_AA);
		
		if( (j%2)==0 )
		{
			rectangle(dsp,Point(w*j,0),Point(w*(j+1),dsp.rows),Scalar::all(150),-1,CV_AA);
		}
		
		for(int t=0;t<timemax;t++){
			int x = T[j].v[t][0] * w + w*j;
			if(T[j].v[t][2]>0){
				dsp.at<Vec3b>(t*1.0/timemax*h,x) = Vec3b(0,255,0);
				//circle(dsp,Point(x,t*1.0/timemax*h),1,CV_RGB(0,255,0),-1,CV_AA);
			}
		}
	}
	
	logpH = computePrior();
	logpDH=0;
	for(int j=0;j<(int)T.size();j++) logpDH += logpDHj[j];
	logpHD = logpH +logpDH;
	stringstream ss;
	//ss << "posterior=" << logpHD << "/"<< best << " (" << m << ")";
	ss << "posterior=" << logpHD;
	putText(dsp, ss.str(),Point(10,40),0,1.0,CV_RGB(255,255,0),1,CV_AA);
	
	imshow("dsp",dsp);
	waitKey(1);
}

double gda::factorial (double a){
	if (a > 1) return (a * factorial (a-1));
	else return (1);
}

double gda::computePrior(){
	double logpH = log(factorial(T.size()));
	for(int j=0;j<(int)T.size();j++){
		if(Tj[j]>0) logpH +=  Tj[j] * log(Tj[j]*1.0/D) + log(pc[C[j]]);
	}
	return logpH;
}

void gda::computeLikelihood(){
	if(VERBOSE) cout << "Method: computeLikelihood" << endl;
	double w,_w,h,_h,x,_x,y,_y;
	int _t=0;
	if(VERBOSE) cout << "Number of trajectories:" << T.size() << endl;
	
	for(int j=0;j<(int)T.size();j++){
		logpDHj[j] = 0;
		
		for(int t=0;t<timemax;t++){
			if(T[j].v[t][2]<=0) continue;
			
			x = T[j].v[t][0];
			y = T[j].v[t][1];
			w = T[j].v[t][2];
			h = T[j].v[t][3];
			
			if(logpDHj[j]<0){ // first entry
				logpDHjt[j][t] = getLogPss(w,_w,h,_h,C[j]);			
				logpDHjt[j][t] += getLogPxx(x,_x,y,_y,t,_t,C[j]);
				logpDHj[j] += logpDHjt[j][t];
			}
			else{
				//cout << j <<" logpDHjt[j].size:" << logpDHjt[j].size() << ">" << t << endl;
				logpDHjt[j][t] = getLogPs(w); 
				logpDHj[j] += logpDHjt[j][t];
			}
			_x = x;
			_y = y;
			_w = w;
			_h = h;
			_t = t;
		}
	}
}


void gda::writeToFile_csv(string out_path, string object_type)
{
	
	cout << "Write to output file..." << endl;
	
	ofstream out;
	
	out.open(out_path.c_str());
	
	if(!out.is_open()){
		cout << "ERROR: Could not output file:" << out_path << endl;
		exit(1);
	}
	else cout << out_path << endl;
	
	out << "Tracker Output\n";
	out << "Screen Aspect = 1.777778e+00\n";
	out << "Frame,Index,LastTrackedFrame,CX,CY,W,H,VelX,VelY,Type\n";
	
	double x,y,w,h,p;
	int f;
	string ob_type;
	
	for(int t=0;t<timemax;t++){
		f = t;
		for(int j=0;j<(int)_T.size();j++){
			
			if(_T[j].v[t][2]<=0) continue;
			
			x = _T[j].v[t][0];			// center x
			y = _T[j].v[t][1]+_T[j].v[t][3]*0.5;	// upper left to center y
			w = _T[j].v[t][2];
			h = _T[j].v[t][3];
			p = _T[j].v[t][4];
			
			out << f+1 << ",";	// frame (zero-based --> one-based)
			out << min_gid+j << ",";	// id
			out << f+1 << ",";	// LastTrackedFrame
			out << x << ",";
			out << y << ",";
			out << w << ",";
			out << h << ",";
			out << -1 << ",";	// velocity
			out << -1 << ",";	// velocity
			
			if(p==0)		ob_type = "person_walk";
			else if(p==1)	ob_type = "person_sitting_on_object";
			else if(p==2)	ob_type = "person_squatting";
			else if(p==3)	ob_type = "person_standing_in_place";
			else if(p==4)	ob_type = "person_straddling_object";
			else if(p==5)	ob_type = "person_bend_over";
			else			ob_type = object_type;
			
			out << ob_type;
			out << endl;
			
			
		}
	}
	out.close();
	
	cout << "Done writing file." << endl;
}

void gda::writeToFile(string basename){
	
	stringstream fs;
	//fs << "/VIRAT/tracks/data_assoc/" << basename << "_data_assoc.txt";
	//fs << basename << "_data_assoc.txt";
	fs << "../../../mey2_data/" + basename + "_data_assoc.txt";
	ofstream out(fs.str().c_str());
	if(!out.is_open()){
		cout << "ERROR: Could not output file:" << fs.str() << endl;
		exit(1);
	}
	
	double x,y,w,h;
	int f,c;
	for(int t=0;t<timemax;t++){
		f = t;
		for(int j=0;j<(int)_T.size();j++){
			
			if(_T[j].v[t][2]<=0) continue;
			
			x = _T[j].v[t][0] - _T[j].v[t][2]*0.5;
			y = _T[j].v[t][1];
			w = _T[j].v[t][2];
			h = _T[j].v[t][3];
			
			c = j;
			
			out << f << " ";
			out << x << " ";
			out << y << " ";
			out << w << " ";
			out << h << " ";
			out << c << " ";
			out << endl;
			
		}
	}
	out.close();
}

void gda::loadData(string input_file, vector <vector< struct rect > > &ped, int &timemax)
{
	
	stringstream tin;
	tin << input_file;
	ifstream fin(tin.str().c_str());
	if(!fin.is_open()) if(VERBOSE) cout << "ERROR: can't open " << tin.str() << endl;
	
	D = 0;				// global variable
	
	timemax = 0;
	
	int k=0;
	double d[5];
	while(fin >> d[k++]){
		if(k==5){
			int idx = round(d[0]);
			rect r;
			r.v[0] = d[1]+d[3]*0.5;
			r.v[1] = d[2];
			r.v[2] = d[3]; 
			r.v[3] = d[4];
			if(idx>MAX_FRAMES){
				cout << "ERROR: More frames than allocated:" << idx << endl;
				exit(1);
			}
			ped[idx].push_back(r);
			if(idx>timemax) timemax = idx;
			k=0;
			D++;
		}
	}
	
	cout << "Load data" << endl;
	cout << "max time is: " << timemax << endl;
}

void gda::loadData_csv(string input_file, vector <vector< struct rect > > &ped, int &timemax, string object_type)
{
	
	ifstream fin(input_file.c_str());
	if(!fin.is_open()) if(VERBOSE) cout << "ERROR: can't open " << input_file << endl;
	cout << "Opening: " << input_file << endl;
	
	D = 0;				// global variable
	timemax = 0;
	string d[10];
	string line;
	
	if(0){
		getline(fin,line);
		cout << "header(1):" << line << endl;
		getline(fin,line);
		cout << "header(2):" << line << endl;
		getline(fin,line);
		cout << "header(3):" << line << endl;
	}
	
	while( getline(fin,line) )
	{
		istringstream ss(line);
		int k=0;
		
		while( getline(ss,d[k++],',') )
		{
			if(k==10)
			{
				
				//if(d[9]!=object_type) continue;
				
				if( d[9].find(object_type) == string::npos) continue; // no string partial match
				
				//cout << "[" << d[9] << "]" << endl;
				stringstream is;
				
				int f    = atoi(d[0].c_str())-1;	// frame number (one based-> zero based)
				int gid  = atoi(d[1].c_str());		// frame number
				double x = atof(d[3].c_str());		// center x
				double y = atof(d[4].c_str());		// center y
				double w = atof(d[5].c_str());		// width
				double h = atof(d[6].c_str());		// height
				
				double pose;
				
				if(d[9]=="person_walk")						pose = 0;
				else if(d[9]=="person_sitting_on_object")	pose = 1;
				else if(d[9]=="person_squatting")			pose = 2;
				else if(d[9]=="person_standing_in_place")	pose = 3;
				else if(d[9]=="person_straddling_object")	pose = 4;
				else if(d[9]=="person_bend_over")			pose = 5;
				else										pose = -1;
				
				rect r;
				r.v[0] = x;			// center x to center x
				r.v[1] = y-h*0.5;	// center y to upper left y
				r.v[2] = w;			// width
				r.v[3] = h;			// height
				r.gid = gid;		// global id
				r.v[4] = pose;		// storing pose id here!!!!
				
				if( gid < min_gid ) min_gid = gid;
				if( gid > max_gid ) max_gid = gid;
				
				if(VERBOSE) cout << "f:" << f << "[gid:" << gid << "] (" << x << "," << y << "," << w << "," << h << ")" << endl; 
				
				if(f>MAX_FRAMES)
				{
					cout << "ERROR: More frames than allocated:" << f << endl;
					exit(1);
				}
				
				ped[f].push_back(r);
				
				if( f>timemax ) timemax = f;
				
				D++;
			}
		}
	}
	
	cout << "Done load data." << endl;
	cout << "max time is: " << timemax << endl;
	
}


//void gda::computeColorHist_HS(Mat src, Mat mask, Mat &hist){
//	
//	// original version with PCA 100
//	
//	if(src.channels()!=3) exit(1);
//	
//	Mat emask;
//	erode(mask,emask,Mat(),Point(-1,-1),1,BORDER_CONSTANT);
//	Mat tmp;
//	src.copyTo(tmp,emask);
//	
//	Mat hsv;
//	cvtColor(src,hsv,CV_BGR2HSV_FULL);
//	int hbins = 64, sbins = 32;
//    int histSize[] = {hbins, sbins};
//    float hranges[] = { 0, 256 };
//    float sranges[] = { 0, 256 };
//    const float* ranges[] = { hranges, sranges };
//    int channels[] = {0, 1};
//	Mat im[2];
//	im[0] = hsv(Rect(0,0,hsv.cols,hsv.rows*0.5));
//	im[1] = hsv(Rect(0,hsv.rows*0.5,hsv.cols,hsv.rows*0.5));
//	Mat mk[2];
//	mk[0] = mask(Rect(0,0,hsv.cols,hsv.rows*0.5));
//	mk[1] = mask(Rect(0,hsv.rows*0.5,hsv.cols,hsv.rows*0.5));
//	Mat hi[2];
//	calcHist( &im[0], 1, channels, mk[0], hi[0], 2, histSize, ranges,true,false);
//	calcHist( &im[1], 1, channels, mk[1], hi[1], 2, histSize, ranges,true,false);
//	hi[0].copyTo(hist);
//	hist.push_back(hi[1]);
//	hist = hist.reshape(0,1);
//	normalize(hist,hist,1,0, NORM_L1);
//}

void gda::computeColorHist_HS(Mat src, Mat mask, Mat &feature)
{
	
	if(src.channels()!=3) exit(1);
	
	Mat hsv;
	cvtColor(src,hsv,CV_BGR2HSV_FULL);
	int hbins = 64, sbins = 8;				// 64,16
    int histSize[] = {hbins, sbins};
    float hranges[] = { 0, 256 };
    float sranges[] = { 0, 256 };
    const float* ranges[] = { hranges, sranges };
    int channels[] = {0, 1};
	
	Rect top = Rect(0,0,hsv.cols,hsv.rows*0.6);
	Rect bot = Rect(0,hsv.rows*0.4,hsv.cols,hsv.rows*0.6);
	//Rect all = Rect(0,0,hsv.cols,hsv.rows);
	
	Mat im[3];
	im[0] = hsv(top);				
	im[1] = hsv(bot);
	//im[2] = hsv(all);
	
	Mat mk[3];
	mk[0] = mask(top);
	mk[1] = mask(bot);
	//mk[2] = mask(all);
	//mk[0] = 255;
	//mk[1] = 255;
	
	Mat hi[3];
	calcHist( &im[0], 1, channels, mk[0], hi[0], 2, histSize, ranges,true,false);
	calcHist( &im[1], 1, channels, mk[1], hi[1], 2, histSize, ranges,true,false);
	//calcHist( &im[2], 1, channels, mk[2], hi[2], 2, histSize, ranges,true,false);
	
	normalize(hi[0],hi[0],1,0, NORM_L1);
	normalize(hi[1],hi[1],1,0, NORM_L1);
	//normalize(hi[2],hi[2],1,0, NORM_L1);
	
	Mat hist;
	hi[0].copyTo(hist);
	hist.push_back(hi[1]);
	//hist.push_back(hi[2]);
	
	hist = hist.reshape(0,1);
	
	normalize(hist,hist,1,0, NORM_L1);
	
	hist.copyTo(feature);
}


void gda::computeColorHist_HSV(Mat src, Mat mask, Mat &hist)
{
	computeColorHist_HS(src,mask,hist);
}


//void gda::computeColorHist_HSV(Mat src, Mat mask, Mat &hist)
//{
//	
//	if(src.channels()!=3) exit(1);
//	
//	int bins[3];
//	bins[0] = 64;	// 64
//	bins[1] = 8;	// 8
//	bins[2] = 1;	// 1
//	
//	Mat tmp;
//	src.copyTo(tmp);
//	Mat hsv;
//	cvtColor(src,hsv,CV_BGR2HSV_FULL);
//	
//	int histSize[] = {bins[0], bins[1], bins[2]};
//	Mat his;
//	his.create(3, histSize, CV_32F);
//    his = Scalar(0);	
//    CV_Assert(hsv.type() == CV_8UC3);
//    MatConstIterator_<Vec3b> it = hsv.begin<Vec3b>();
//	MatConstIterator_<Vec3b> it_end = hsv.end<Vec3b>();
//    for( ; it != it_end; ++it )
//    {
//        const Vec3b& pix = *it;
//        his.at<float>(pix[0]*bins[0]/256, pix[1]*bins[1]/256, pix[2]*bins[2]/256) += 1.f;
//    }
//	
//	// ==== Remove small values ==== //
//	float minProb = 1.0 / bins[0]*bins[1]*bins[2] * 0.01; //0.01;
//    minProb *= hsv.rows*hsv.cols;
//    Mat plane;
//	const Mat *_his = &his;
//	
//    NAryMatIterator itt = NAryMatIterator(&_his, &plane, 1);	
//	threshold(itt.planes[0], itt.planes[0], minProb, 0, THRESH_TOZERO);
//	double s = sum(itt.planes[0])[0];
//    
//	// ==== Normalize ==== //
//	s = 1./s * 255.;
//	itt.planes[0] *= s;
//	
//	itt.planes[0].copyTo(hist);
//	
//}